#include "raster.h"
#include "raster-fixed.h"
#include "raster-fixed-math.h"
#include "raster-channels.h"
#include "raster-util.h"
#include <stdlib.h>

extern int __cdecl LineBlitXRGB(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPos *sPos, BlitMode *mode);
extern int __cdecl LineBlitARGB(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPos *sPos, BlitMode *mode);
int __cdecl LineBlitShade(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPos *sPos, BlitMode *mode);
int __cdecl LineBlitError(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPos *sPos, BlitMode *mode)
{
    return ERROR_INVALID_INPUT; /* invalid format */
}


extern int __cdecl LineBlitUVXRGB(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPosUV *leftUV, const RasterPosUV *rightUV, BlitMode *mode);
extern int __cdecl LineBlitUVARGB(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPosUV *leftUV, const RasterPosUV *rightUV, BlitMode *mode);
int __cdecl LineBlitUVShade(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPosUV *leftUV, const RasterPosUV *rightUV, BlitMode *mode);
int __cdecl LineBlitUVError(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPosUV *leftUV, const RasterPosUV *rightUV, BlitMode *mode)
{
    return ERROR_INVALID_INPUT; /* invalid format */
}

extern int __cdecl SpanBlitXRGB(Raster *dest, Raster *src, int y, RasterSpan *span, BlitMode *mode);
extern int __cdecl SpanBlitARGB(Raster *dest, Raster *src, int y, RasterSpan *span, BlitMode *mode);
int __cdecl SpanBlitShade(Raster *dest, Raster *src, int y, RasterSpan *span, BlitMode *mode);
int __cdecl SpanBlitError(Raster *dest, Raster *src, int y, RasterSpan *span, BlitMode *mode)
{
    return ERROR_INVALID_INPUT;
}

RasterFormat builtinFormats[FORMAT_HIGHEST+1] =
    {
        /* FORMAT_FLAT_XRGB */
        {
            4,
            0x00000000,
            0,
            0x00FF0000,
            16,
            0x0000FF00,
            8,
            0x000000FF,
            0,
            NULL,
            LineBlitXRGB,
            LineBlitUVXRGB,
            SpanBlitXRGB
        },
        /* FORMAT_FLAT_XRGB */
        {
            4,
            0xFF000000,
            24,
            0x00FF0000,
            16,
            0x0000FF00,
            8,
            0x000000FF,
            0,
            NULL,
            LineBlitARGB,
            LineBlitUVARGB,
            SpanBlitARGB
        },
        /* FORMAT_FLAT_SHADE */
        {
            0,
            0, 0, 0, 0, 0, 0, 0, 0, /* no color information */
            NULL,
            LineBlitShade,
            LineBlitUVShade,
            SpanBlitShade
        }
    };

RasterFormat *userFormats = NULL;
size_t ctUserFormats = 0;
size_t szUserFormats = 0;

const RasterFormat __cdecl LookupFormat(unsigned int fmt)
{
    RasterFormat ret;
    ret.LineBlit = LineBlitError;
    if(fmt <= FORMAT_HIGHEST)
        ret = builtinFormats[fmt];
    else if(fmt >= FORMAT_USER)
    {
        fmt -= FORMAT_USER;
        if(fmt <= ctUserFormats)
        {
            /* todo: lock here */
            ret = userFormats[fmt];
            /* todo: unlock here */
        }
    }

    return ret;
}

unsigned int __cdecl RegisterUserFormat(RasterFormat *fmt)
{
    unsigned int ret;
    /* todo: lock here */
    if(userFormats == NULL)
    {
        /* allocate 8 raster formats initially */
        userFormats = (RasterFormat *)malloc(sizeof(RasterFormat) * 8);
        szUserFormats = 8;
    }
    ret = FORMAT_USER + ctUserFormats;
    userFormats[ctUserFormats++] = *fmt;
    /* todo: unlock here */
    return ret;
}

int __cdecl LineBlitShade(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPos *sPos, BlitMode *mode)
{
    Pixel *dp = dest->pixels + (dPos->y * dest->pitch) + dPos->x;
    signed char *sp = (signed char *)src->pixels + (sPos->y * src->pitch) + sPos->x;
    Pixel highlight =   0xFFFFFFFF; /* white */
    Pixel shadow =      0xFF000000; /* black */

    if(mode)
    {
        if(mode->modes == BLIT_COLORMOD)
        {
            highlight = mode->mod.color.wht;
            shadow = mode->mod.color.blk;
        }
        else
            return ERROR_INVALID_INPUT; /* this format does not accept any blit modes */
    }


    while(num)
    {
        /* quick and simple, may not achieve desirable results? */
        if(*sp != 0)
        {
            Pixel color;
            if(*sp > 0)
                color = highlight | (((*sp) * (highlight >> 24)) / 255) << 24;
            else
                color = shadow | ((-(*sp) * (highlight >> 24)) / 255) << 24;
            *dp = AlphaBlendPixels(*dp, color);
        }

        sp++;
        dp++;
        num--;
    }
    return 0;
}

#define TEST_COLORKEY   1
#define TEST_ALPHA      2

#define COLORCHANS(pel) pel & 0xFF, (pel >> 8) & 0xFF, (pel >> 16) & 0xFF, (pel >> 24) & 0xFF

/* 64-bit processors always have SSE2 so just use intrinsics */
#if defined(__x86_64__) || defined(_M_AMD64)
#include <emmintrin.h>
Pixel __cdecl _InterpolateBilinearSSE2(Pixel *pels, Half *mods, int testmode, Pixel colorkey)
{
    __m128i ccret = _mm_setzero_si128();
    __m128i ccavg = _mm_setzero_si128();
    int pretest[4] = {0,0,0,0};
    int i = 0;

    /* although this could be optimized for SSE2, pels being unaligned makes it slower. */
    if(testmode)
    {
        if(testmode & BILINEAR_TEST_COLORKEY)
        {
            pretest[0] |= pels[0] == colorkey;
            pretest[1] |= pels[1] == colorkey;
            pretest[2] |= pels[2] == colorkey;
            pretest[3] |= pels[3] == colorkey;
        }
        else colorkey = 0;
        if(testmode & BILINEAR_TEST_ALPHA)
        {
            pretest[0] |= (pels[0] & PIXEL_TARGET_ALPHA_MASK) == 0;
            pretest[1] |= (pels[1] & PIXEL_TARGET_ALPHA_MASK) == 0;
            pretest[2] |= (pels[2] & PIXEL_TARGET_ALPHA_MASK) == 0;
            pretest[3] |= (pels[3] & PIXEL_TARGET_ALPHA_MASK) == 0;
        }

        testmode = (pretest[0] || pretest[1] || pretest[2] || pretest[3]);
    }

    if(testmode)
    {
        Half merged = 0;
        int numavg = 0;
        while(i < 4)
        {
            if(!pretest[i])
            {
                __m128i pelcc = _mm_cvtsi32_si128(pels[i]);
                pelcc = _mm_unpacklo_epi8(pelcc, _mm_setzero_si128());
                ccavg = _mm_add_epi16(ccavg, pelcc);
                numavg++;

                pelcc = _mm_mullo_epi16(pelcc, _mm_set1_epi16(mods[i]));
                ccret = _mm_add_epi16(ccret, pelcc);
                merged += mods[i];
            }
            ++i;
        }

        else if(numavg != 4)
        {
            Half unmerged = (HALF_ONE - merged) / numavg;
            if(numavg == 0)
                return colorkey;
            ccavg = _mm_mullo_epi16(ccavg, _mm_set1_epi16((short)unmerged));
            ccavg = _mm_insert_epi16(ccavg, 0, 3);
            ccret = _mm_add_epi16(ccret, ccavg);
        }
    }
    else
    {
        while(i < 4)
        {
            __m128i pelcc = _mm_cvtsi32_si128(pels[i]);
            pelcc = _mm_unpacklo_epi8(pelcc, _mm_setzero_si128());
            pelcc = _mm_mullo_epi16(pelcc, _mm_set1_epi16(mods[i]));
            ccret = _mm_add_epi16(ccret, pelcc);
            ++i;
        }
    }

    ccret = _mm_srli_epi16(ccret, 8);
    return _mm_cvtsi128_si32(_mm_packus_epi16(ccret, ccret));
}
#elif defined(__i386) || defined(_M_IX86)
/* only newer IA-32 devices have SSE2, so use assembler routines and an auto-chooser routine to ensure
    reliably reusable builds */
extern int __cdecl supportsSSE2();
extern Pixel __cdecl _InterpolateBilinearSSE2(Pixel *pels, Half *mods, int testmode, Pixel colorkey);
#endif /* __SSE2__*/
Pixel __cdecl _InterpolateBilinear(Pixel *pels, Half *mods, int testmode, Pixel colorkey)
{
    static const unsigned int CHANMASK0 = 0x00FF00FF;
    static const unsigned int CHANMASK1 = 0xFF00FF00;
    unsigned int retrb = 0, retag = 0;
    int pretest[4] = {0,0,0,0};
    int i = 0;
    if(testmode)
    {
        if(testmode & BILINEAR_TEST_COLORKEY)
        {
            pretest[0] |= pels[0] == colorkey;
            pretest[1] |= pels[1] == colorkey;
            pretest[2] |= pels[2] == colorkey;
            pretest[3] |= pels[3] == colorkey;
        }
        else colorkey = 0;
        if(testmode & BILINEAR_TEST_ALPHA)
        {
            pretest[0] |= (pels[0] & PIXEL_TARGET_ALPHA_MASK) == 0;
            pretest[1] |= (pels[1] & PIXEL_TARGET_ALPHA_MASK) == 0;
            pretest[2] |= (pels[2] & PIXEL_TARGET_ALPHA_MASK) == 0;
            pretest[3] |= (pels[3] & PIXEL_TARGET_ALPHA_MASK) == 0;
        }

        testmode = (pretest[0] || pretest[1] || pretest[2] || pretest[3]);
        /* note:
            precomputing a completely failed test here is a hair slower than letting the loop run
        */
    }

    if(testmode)
    {
        Half unmerged = HALF_ONE; /* prevent darkening from color key or transparency */
        int numavg = 0;
        unsigned int avgrb = 0, avgag = 0;
        while(i < 4)
        {
            if(!pretest[i])
            {
                unsigned int pelrb = pels[i] & CHANMASK0;
                unsigned int pelag = (pels[i] >> 8) & CHANMASK0;
                retrb += pelrb * mods[i];
                retag += pelag * mods[i];
                avgrb += pelrb;
                avgag += pelag;
                unmerged -= mods[i];
                numavg++;
            }

            ++i;
        }
        if(numavg != 4)
        {
            Half mul;
            if(numavg == 0)
                return colorkey;
            mul = unmerged / numavg;
            retrb += (avgrb * mul);
            retag += ((avgag & 0xFFFF /* mask out alpha */) * mul);
        }
    }
    else
    {
        /*while(i < 4)
        {
            retrb += (pels[i] & CHANMASK0) * mods[i];
            retag += ((pels[i] >> 8) & CHANMASK0) * mods[i];
            ++i;
        }
        this loop is simple enough to just unroll */
        retrb += (pels[0] & CHANMASK0) * mods[0];
        retag += ((pels[0] >> 8) & CHANMASK0) * mods[0];
        retrb += (pels[1] & CHANMASK0) * mods[1];
        retag += ((pels[1] >> 8) & CHANMASK0) * mods[1];
        retrb += (pels[2] & CHANMASK0) * mods[2];
        retag += ((pels[2] >> 8) & CHANMASK0) * mods[2];
        retrb += (pels[3] & CHANMASK0) * mods[3];
        retag += ((pels[3] >> 8) & CHANMASK0) * mods[3];

    }
    return (Pixel)(((retrb & CHANMASK1) >> 8) | (retag & CHANMASK1));
}

static Pixel __cdecl _interpolateChooser(Pixel *pels, Half *mods, int testmode, Pixel colorkey);
Pixel (__cdecl *InterpolateBilinear)(Pixel *pels, Half *mods, int testmode, Pixel colorkey) = _interpolateChooser;

static Pixel __cdecl _interpolateChooser(Pixel *pels, Half *mods, int testmode, Pixel colorkey)
{
    #if defined(__x86_64__) || defined(_M_AMD64)
    InterpolateBilinear = _InterpolateBilinearSSE2;
    #elif defined(__i386) || defined(_M_IX86)
    if(supportsSSE2())
        InterpolateBilinear = _InterpolateBilinearSSE2;
    else
        InterpolateBilinear = _InterpolateBilinear;
    #else
    InterpolateBilinear = _InterpolateBilinear;
    #endif
    return InterpolateBilinear(pels, mods, testmode, colorkey);
}

BEST_INLINE unsigned char _InterpolatePixelShade(Raster *src, RasterPosUV *uv, int mode)
{
    /* nearest-neighbor */
    if(mode == BLIT_INTERPOLATE_BILINEAR)
    {
        /* pixel offsets */
        unsigned char *v0 = (unsigned char *)src->pixels + (src->pitch * FixedToInt(uv->v));
        unsigned char *v1 = (unsigned char *)src->pixels + (src->pitch * IMIN(src->height - 1, FixedToInt(uv->v) + 1));
        int u0 = FixedToInt(uv->u);
        int u1 = IMIN(src->width - 1, FixedToInt(uv->u) + 1);
        /* pixel proportions */
        Fixed uf = (uv->u & 0xFFFF);
        Fixed uo = FIXED_ONE - uf;
        Fixed vf = (uv->v & 0xFFFF);
        Fixed vo = FIXED_ONE - vf;
        /* looping -
            todo: using lookup table for reduced-precision mods might be worthwhile */
        Fixed mods[4] = { FixedMulLossy(uo, vo), FixedMulLossy(uf, vo), FixedMulLossy(uo, vf), FixedMulLossy(uf, vf) };
        int pels[4] = { *(v0 + u0), *(v0 + u1), *(v1 + u0), *(v1 + u1) };
        return (unsigned char)FixedToInt(((pels[0] * mods[0]) + (pels[1] * mods[1]) + (pels[2] * mods[2]) + (pels[3] * mods[3])) / 4);
    }
    int u = FixedToInt(uv->u + FIXED_HALF);
    int v = FixedToInt(uv->v + FIXED_HALF);
    const unsigned char *pixels = (const unsigned char *)(src->pixels);
    return *(pixels + (v * src->pitch) + u);
}


int __cdecl LineBlitUVShade(Raster *dest, Raster *src, unsigned int num, const RasterPos *dPos, const RasterPosUV *leftUV, const RasterPosUV *rightUV, BlitMode *mode)
{
    Pixel *dp = dest->pixels + (dPos->y * dest->pitch) + dPos->x;
    Pixel highlight =   0xFFFFFFFF; /* white */
    Pixel shadow =      0xFF000000; /* black */
    RasterPosUV sPos = *leftUV;
    /* num-1 seems to produce a better result here */
    Fixed uIter = (rightUV->u - leftUV->u) / IMAX(1, (num - 1));
    Fixed vIter = (rightUV->v - leftUV->v) / IMAX(1, (num - 1));
    int interpol = 0;

    if(mode)
    {
        if(mode->modes == BLIT_COLORMOD)
        {
            highlight = mode->mod.color.wht;
            shadow = mode->mod.color.blk;
        }
        else
            return ERROR_INVALID_INPUT; /* this format does not accept any blit modes */
        interpol = mode->interpolation;
    }


    while(num)
    {
        /* quick and simple, may not achieve desirable results? */
        unsigned char shade = _InterpolatePixelShade(src, &sPos, interpol);
        if(shade != 0)
        {
            Pixel color;
            if(shade > 0)
                color = highlight | (((shade) * (highlight >> 24)) / 255) << 24;
            else
                color = shadow | ((-(shade) * (highlight >> 24)) / 255) << 24;
            *dp = AlphaBlendPixels(*dp, color);
        }

        sPos.u += uIter;
        sPos.v += vIter;
        dp++;
        num--;
    }
    return 0;
}

int __cdecl SpanBlitShade(Raster *dest, Raster *src, int y, RasterSpan *span, BlitMode *mode)
{
    return 0; /* todo */
}
